//initialisatie functies
void sendstartschot(); // startschot versturen
int receiveIDs(); //IDs van auto's ontvangen
void setIDs(int maxid); //zelf IDs aan auto's toekennen
void givedistance(int maxid); //zeggen dat ze data moeten sturen

//main functies
void drive(int id); // auto's laten rijden
int check(int maxid); //data controleren
void send(); //data versturen naar aarde + crickets
void inlezen();


void char2bin(unsigned char number, bool * bitarray);
char bin2char(bool * bitarray);
void encode(unsigned char number, bool * bitarray1, bool * bitarray2);
bool decode(unsigned char * number, bool * bitarray1, bool * bitarray2);

//globale variabelen
char codeddata[1000]; //data die we binnenkrijgen
char data[20]; //data die gedecodeerd is
char newdata[32]; //data die we gaan versturen


void main()
{
    //variabelen
    int id = 0; //current id van auto
    int maxid = 0; //maximaal aantal auto's
    int drivecount = 0; //hoeveel keer auto al heeft gereden na foute meting
    unsigned char bitarray1;
    unsigned char bitarray2;
    unsigned char number;

    //initialisatie
    sendstartschot();
    maxid = receiveIDs();
    setIDs(maxid);
    int a;
    for(a = 0; a<maxid; a++)
    {
        drive(id);
    }
    int b;
    for(b = 0; b<maxid; b++)
    {
        givedistance(b);
        // decoderen hier + opslaan
        if(check(maxid) == 1)
        {
            encode();
            send();
        }
        else
        {
            givedistance(maxid);
            // decoderen + opslaan
            if(check(maxid) == 1)
            {
                encode();
                send();
            }
        }
    }


    //normale loop
    while(1)
    {
        inlezen();
        decode();
        bool tjek = check(maxid);
        if(tjek == 1) //goede data
        {
            send();
            if(id == maxid)
            {
                id = 0;
            }
            else
            {
                id = id + 1;
            }
            drive(id);
        }
        else if(tjek == 0)
        {
            if(drivecount < 3)
            {
                drive(id);
                drivecount = drivecount + 1;
            }
            else
            {
                drivecount = 0;
                if(id == maxid)
                {
                    id = 0;
                }
                else
                {
                    id = id + 1;
                }
                drive(id);
            }
        }  
    }
}

//initialisatie functies
void sendstartschot(){
    bool bitarray1[8];
    bool bitarray2[8];
    bool *rs232_port = (bool*)0xC00000;//set memory
    bool a[8] = {1,0,0,0,0,0,0,0};
    unsigned char startschot = bin2char(a);
    encode(startschot,bitarray1,bitarray2);
    char b = bin2char(bitarray1);
    char c = bin2char(bitarray2);
    rs232_port[0] = b;
    rs232_port[0] = c; /* write to port */
    rs232_port[0] = '\r';
    rs232_port[0] = '\n';
}
int receiveIDs(){

}
void setIDs(int maxid){
    bool *rs232_port = (bool*)0xC00000;//set memory

    unsigned char a;
    int i = 0;
    for(i=0;i<maxid;i++)
    {
        rs232_port[0] = a; 
        unsigned char a = ;//next id
    }
}
void givedistance(int maxid){
    bool *rs232_port = (bool*)0xC00000;//set memory

    unsigned char a = 0;
    int id = 0;
    for(id = 0;id<maxid;id++)
    {
        rs232_port[0] = ; // bericht voor iedere cricket anders
    }
}


//main functies
void drive(int id){
    bool bitarray[8];
    bool bitarray1[8];
    bool bitarray2[8];
    bool *rs232_port = (bool*)0xC00000;//set memory
    char2bin(id,bitarray);
    bool a[8] = {1,1,bitarray[4],bitarray[5],bitarray[6],bitarray[7],0,0};
    unsigned char drivecode = bin2char(a);//??
    encode(drivecode,bitarray1,bitarray2);
    unsigned char b = bin2char(bitarray1);
    unsigned char c = bin2char(bitarray2);
    rs232_port[0] = b;/* write to port */
    rs232_port[0] = c;
    rs232_port[0] = '\r';
    rs232_port[0] = '\n';
}
void inlezen()
{
    int a = 0;
    bool *rs232_port = (bool*)0xC00000;//set memory
    for(a=0;a<9;a++) // 9 want STX - Bericht(2 bytes voor afstand + 4 bits voor id) - ETX
    {
        unsigned char b = 0;
        int i = 0;
        b = rs232_port[0];
        codeddata[a+i] = b;
        if(b = 'DLE')
        {
            a=a-1;
            i = i+1;
        }
    }
}
int check(int maxid){
    data[maxid] = ;//adress
    int errors = 0;
    int i = 0;
    for(i = 0; i < maxid; i++)
    {
        if(data[i] < 20)
        {
            data[i] = 255;
            errors = errors + 1;
        }
        else if(data[i] > 200)
        {
            data[i] = 255;
            errors = errors +1;
        }
    }
    if(errors > maxid-3)
    {
        return 0;
    }
    else
    {
        return 1;
    }
}
void send(){
    bool *rs232_port = (bool*)0xC00000;//set memory

    unsigned char a = //adress;
    rs232_port[0] = a; /* write to port */
}

void char2bin(unsigned char number, bool * bitarray) {
    for (int j=0; j<8; j++) {
        bitarray[j] = ((number >> j) & 1);
    }
}

char bin2char(bool * bitarray) {
    char number = 0;

    for(int i=0; i<8; i++) {
        if(bitarray[i]) {
            number += 1<<i;
        }
    }
    return number;
}

void encode(unsigned char number, bool * bitarray1, bool * bitarray2) {
    //b3, b3^b2^b1, b2, !b2^b1^b0, b1, !b3^b1^b0, b0, !b3^b2^b0
    bool bitarray[8];
    char2bin(number, bitarray);

    bitarray1[7] = bitarray[3];
    bitarray1[6] = bitarray[3]^bitarray[2]^bitarray[1];
    bitarray1[5] = bitarray[2];
    bitarray1[4] = !bitarray[2]^bitarray[1]^bitarray[0];
    bitarray1[3] = bitarray[1];
    bitarray1[2] = !bitarray[3]^bitarray[1]^bitarray[0];
    bitarray1[1] = bitarray[0];
    bitarray1[0] = !bitarray[3]^bitarray[2]^bitarray[0];

    bitarray2[7] = bitarray[7];
    bitarray2[6] = bitarray[7]^bitarray[6]^bitarray[5];
    bitarray2[5] = bitarray[6];
    bitarray2[4] = !bitarray[6]^bitarray[5]^bitarray[4];
    bitarray2[3] = bitarray[5];
    bitarray2[2] = !bitarray[7]^bitarray[5]^bitarray[4];
    bitarray2[1] = bitarray[4];
    bitarray2[0] = !bitarray[7]^bitarray[6]^bitarray[4];
}

bool decode(unsigned char * number, bool * bitarray1, bool * bitarray2) {
    //    p  = h7 ^ h6 ^ h5 ^ h4 ^ h3 ^ h2 ^ h1 ^ h0
    //    c0 = h7 ^ h5 ^ h1 ^ h0
    //    c1 = h7 ^ h3 ^ h2 ^ h1
    //    c2 = h5 ^ h4 ^ h3 ^ h1
    bool bitarray[8];
    bool p1, p2, c01, c02, c11, c12, c21, c22;
    
    p1  = bitarray1[7] ^ bitarray1[6] ^ bitarray1[5] ^ bitarray1[4] ^ bitarray1[3] ^ bitarray1[2] ^ bitarray1[1] ^ bitarray1[0];
    c01 = bitarray1[7] ^ bitarray1[5] ^ bitarray1[1] ^ bitarray1[0];
    c11 = bitarray1[7] ^ bitarray1[3] ^ bitarray1[2] ^ bitarray1[1];
    c21 = bitarray1[5] ^ bitarray1[4] ^ bitarray1[3] ^ bitarray1[1];

    p2  = bitarray2[7] ^ bitarray2[6] ^ bitarray2[5] ^ bitarray2[4] ^ bitarray2[3] ^ bitarray2[2] ^ bitarray2[1] ^ bitarray2[0];
    c02 = bitarray2[7] ^ bitarray2[5] ^ bitarray2[1] ^ bitarray2[0];
    c12 = bitarray2[7] ^ bitarray2[3] ^ bitarray2[2] ^ bitarray2[1];
    c22 = bitarray2[5] ^ bitarray2[4] ^ bitarray2[3] ^ bitarray2[1];

    // If the parity, p, is correct (equal to 1) then either 0 or 2 errors occurred. If all the check bits, c0, c1, c2 are correct (equal to 1) then the byte was received intact, (no errors) otherwise it was damaged beyond repair (two errors).
    if((p1 && !(c01 && c11 && c21)) || (p2 && !(c02 && c12 && c22))) {
        return 0;
    }

    // h7, h5, h3, h1
    bitarray[7] = bitarray2[7];
    bitarray[6] = bitarray2[5];
    bitarray[5] = bitarray2[3];
    bitarray[4] = bitarray2[1];
    bitarray[3] = bitarray1[7];
    bitarray[2] = bitarray1[5];
    bitarray[1] = bitarray1[3];
    bitarray[0] = bitarray1[1];

    //     c0 c1 c2    meaning
    //     0  0  1    error in bit h7
    //     0  1  0    error in bit h5
    //     1  0  0    error in bit h3
    //     0  0  0    error in bit h1
    if(!p2) {
        if(!c02 && !c12 && c22)
            bitarray[7] = !bitarray[7];
        if(!c02 && c12 && !c22)
            bitarray[6] = !bitarray[6];
        if(c02 && !c12 && !c22)
            bitarray[5] = !bitarray[5];
        if(!c02 && !c12 && !c22)
            bitarray[4] = !bitarray[4];
        printf("Bit fixed in bitarray2\n");
    }
    if(!p1) {
        if(!c01 && !c11 && c21)
            bitarray[3] = !bitarray[3];
        if(!c01 && c11 && !c21)
            bitarray[2] = !bitarray[2];
        if(c01 && !c11 && !c21)
            bitarray[1] = !bitarray[1];
        if(!c01 && !c11 && !c21)
            bitarray[0] = !bitarray[0];
        printf("Bit fixed in bitarray1\n");
    }

    *number = bin2char(bitarray);
    return 1;
}

/* zendcommando voor cricket en aarde
coderen + decoderen erin zetten
data opslaan erin zetten (als dit gedaan is functies afmaken)